//
// Created by lqk on 2022/2/21.
//

#include "OffsetDurationGait.h"

/**
 *
 * @param nSegment Number of MPC calculations in every gait period
 * @param offsets Offsets of each leg in MPC segments
 * @param stance Duration of stance in MPC segments
 * @param name Gait name
 */
OffsetDurationGait::OffsetDurationGait(int nSegment, Eigen::Vector4i offsets, int stance, const std::string& name):m_offsets(offsets.array()),m_nSegment(nSegment),m_name(name),m_stance(stance){
    m_offsetsFloat=offsets.cast<float>()/(float)nSegment;
    m_stanceFloat=(float)stance/(float)nSegment;
    m_mpcTable=new int[m_nSegment*4];
}

OffsetDurationGait::~OffsetDurationGait() {
    delete[] m_mpcTable;
}
/**
 *
 * @return The phase of contact duration of each leg. If the leg is not in stance, the returned value will be zero.
 */
Eigen::Vector4f OffsetDurationGait::GetStancePhase() {
    Eigen::Array4f progress=m_phase-m_offsetsFloat;
    for(int i=0;i<4;i++){
        if(progress[i]<0) progress[i]+=1.0;
        if(progress[i]>m_stanceFloat){
            progress[i]=0.0;
        }else{
            progress[i]=progress[i]/m_stanceFloat;
        }
    }
    return progress.matrix();
}
/**
 *
 * @return The phase of swing duration of each leg. If the leg is not swinging, the returned value will be zero.
 */
Eigen::Vector4f OffsetDurationGait::GetSwingPhase() {
    Eigen::Array4f swingOffset=m_offsetsFloat+m_stanceFloat;
    for(int i=0;i<4;i++){
        if(swingOffset[i]>1) swingOffset[i]-=1;
    }
    Eigen::Array4f progress=m_phase-swingOffset;
    float swingFloat=1.0-m_stanceFloat;
    for(int i=0;i<4;i++){
        if(progress[i]<0) progress[i]+=1.0;
        if(progress[i]>swingFloat){
            progress[i]=0.0;
        }else{
            progress[i]=progress[i]/swingFloat;
        }
    }
    return progress.matrix();
}

float OffsetDurationGait::GetGaitPhase() {
    return m_phase;
}

float OffsetDurationGait::GetStanceDuration(float dtMPC) {
    return dtMPC*m_stance;
}

float OffsetDurationGait::GetSwingDuration(float dtMPC) {
    return dtMPC*(m_nSegment-m_stance);
}

void OffsetDurationGait::UpdatePhase(int loopsPerMPC, int loopsCounter) {
    m_mpcIteration=(loopsCounter/loopsPerMPC)%m_nSegment;
    m_phase=(float)(loopsCounter%(loopsPerMPC*m_nSegment))/(float)(loopsPerMPC*m_nSegment);
}

int *OffsetDurationGait::GetMPCTable() {
    for(int i=0;i<m_nSegment;i++){
        int iter=(i+m_mpcIteration+1)%m_nSegment;
        Eigen::Array4i progress=iter-m_offsets;
        for(int j=0;j<4;j++){
            if(progress[j]<0) progress[j]+=m_nSegment;
            if(progress[j]<m_stance)
                m_mpcTable[i*4+j]=1;
            else
                m_mpcTable[i*4+j]=0;
//            std::cout<<m_mpcTable[i*4+j];
        }
//        std::cout<<std::endl;
    }
//    std::cout<<std::endl;
    return m_mpcTable;
}
